// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Transform2DTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;
    using System.Drawing;
    using System.Drawing.Drawing2D;
    using System.Linq;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Unit tests for the Transform2D class. 
    /// </summary>
    [TestClass]
    public class Transform2DTest
    {
        /// <summary>
        /// Validate direct transform
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ValidateTransform2D()
        {
            Point2D[] points = { new Point2D(0, 0), new Point2D(1, 1), new Point2D(-2, -1), new Point2D(-3, 10), new Point2D(5, -2) };

            // translation
            {
                Pose2D pose = new Pose2D(-1, 0, 0);
                CoordinateTransform2D translation = new CoordinateTransform2D(pose);
                Matrix matrix = new Matrix();
                matrix.Translate((float)pose.X, (float)pose.Y);
                Point2D[] expectedPoints = BaselineTransform(matrix, points);
                ValidateTransformResults(translation, points, expectedPoints);
            }

            // rotation
            {
                Pose2D pose = new Pose2D(0, 0, 1);
                CoordinateTransform2D rotation = new CoordinateTransform2D(pose);
                Matrix matrix = new Matrix();
                matrix.Rotate((float)(pose.Heading * MathConstants.Radians2Degrees));
                Point2D[] expectedPoints = BaselineTransform(matrix, points);
                ValidateTransformResults(rotation, points, expectedPoints);
            }

            // rotation and translation
            {
                Pose2D pose = new Pose2D(-1, 5, Math.PI / 6);
                CoordinateTransform2D rt = new CoordinateTransform2D(pose);
                Matrix matrix = new Matrix();
                matrix.Rotate((float)(pose.Heading * MathConstants.Radians2Degrees));
                matrix.Translate((float)pose.X, (float)pose.Y, MatrixOrder.Append);
                Point2D[] expectedPoints = BaselineTransform(matrix, points);
                ValidateTransformResults(rt, points, expectedPoints);
            }
        }

        /// <summary>
        /// Validate direct transform of Pose2D
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ValidatePose2DTransform2D()
        {
            Pose2D[] pose2Ds = { new Pose2D(0, 0, 0), new Pose2D(1, 1, Math.PI), new Pose2D(-2, -1, MathConstants.PIOverFour), new Pose2D(-3, 10, MathConstants.PIOverFour * 3), new Pose2D(5, -2, Math.PI / 3) };

            // translation
            {
                Pose2D pose = new Pose2D(-1, 0, 0);
                CoordinateTransform2D translation = new CoordinateTransform2D(pose);
                Matrix matrix = new Matrix();
                matrix.Translate((float)pose.X, (float)pose.Y);                
                Pose2D[] expectedPose2Ds = BaselineTransformPose2D(matrix, 0, pose2Ds);
                ValidatePose2DTransformResults(translation, pose2Ds, expectedPose2Ds);
            }

            // rotation
            {
                Pose2D pose = new Pose2D(0, 0, 1);
                CoordinateTransform2D rotation = new CoordinateTransform2D(pose);
                Matrix matrix = new Matrix();
                double rotationAngle = (pose.Heading);
                matrix.Rotate((float)(rotationAngle * MathConstants.Radians2Degrees));
                Pose2D[] expectedPose2Ds = BaselineTransformPose2D(matrix, rotationAngle, pose2Ds);
                ValidatePose2DTransformResults(rotation, pose2Ds, expectedPose2Ds);
            }

            // rotation and translation
            {
                Pose2D pose = new Pose2D(-1, 5, Math.PI / 6);
                CoordinateTransform2D rt = new CoordinateTransform2D(pose);
                Matrix matrix = new Matrix();
                double rotationAngle = (pose.Heading);
                matrix.Rotate((float)(rotationAngle * MathConstants.Radians2Degrees));
                matrix.Translate((float)pose.X, (float)pose.Y, MatrixOrder.Append);
                Pose2D[] expectedPose2Ds = BaselineTransformPose2D(matrix, rotationAngle, pose2Ds);
                ValidatePose2DTransformResults(rt, pose2Ds, expectedPose2Ds);
            }
        }

        /// <summary>
        /// Validate reverse transform
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ValidateReverseTransform2D()
        {
            Point2D[] points = { new Point2D(0, 0), new Point2D(1, 1), new Point2D(-2, -1), new Point2D(-3, 10), new Point2D(5, -2) };

            // translation
            {
                Pose2D pose = new Pose2D(-1, 0, 0);
                CoordinateTransform2D translation = new CoordinateTransform2D(pose).Reverse();
                Matrix matrix = new Matrix();
                matrix.Translate((float)pose.X, (float)pose.Y);
                Point2D[] expectedPoints = BaselineTransform(matrix, points);
                ValidateTransformResults(translation, expectedPoints, points);
            }

            // rotation
            {
                Pose2D pose = new Pose2D(0, 0, 1);
                CoordinateTransform2D rotation = new CoordinateTransform2D(pose).Reverse();
                Matrix matrix = new Matrix();
                matrix.Rotate((float)(pose.Heading * MathConstants.Radians2Degrees));
                Point2D[] expectedPoints = BaselineTransform(matrix, points);
                ValidateTransformResults(rotation, expectedPoints, points);
            }

            // rotation and translation
            {
                Pose2D pose = new Pose2D(-1, 5, Math.PI / 6);
                CoordinateTransform2D rt = new CoordinateTransform2D(pose).Reverse();
                Matrix matrix = new Matrix();
                matrix.Rotate((float)(pose.Heading * MathConstants.Radians2Degrees));
                matrix.Translate((float)pose.X, (float)pose.Y, MatrixOrder.Append);
                Point2D[] expectedPoints = BaselineTransform(matrix, points);
                ValidateTransformResults(rt, expectedPoints, points);
            }
        }

        /// <summary>
        /// Validates that direct and reverse transforms yield the same point
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ValidateDoubleTransform2D()
        {
            Pose2D origin = new Pose2D(-1.23, 4.56, 0.789);
            Point2D[] points = { new Point2D(0, 0), new Point2D(1, 1), new Point2D(-2, -1), new Point2D(-1, 10), new Point2D(5, -2) };
            CoordinateTransform2D direct = new CoordinateTransform2D(origin);
            CoordinateTransform2D reverse = direct.Reverse();

            Point2D[] transformedPoints = points.Select(p => direct.Transform(p)).ToArray();
            ValidateTransformResults(reverse, transformedPoints, points);

            // also validate matrix multiplication yields identity matrix
            Matrix3 result = direct.GetTransformationMatrix() * reverse.GetTransformationMatrix();
            Assert.IsTrue(result == Matrix3.Identity(), "Not an identity");

            // verify scale as well
            direct = new CoordinateTransform2D(origin, 0.5);
            reverse = direct.Reverse();
            transformedPoints = points.Select(p => direct.Transform(p)).ToArray();
            ValidateTransformResults(reverse, transformedPoints, points);

            // also validate matrix multiplication yields identity matrix
            result = direct.GetTransformationMatrix() * reverse.GetTransformationMatrix();
            Assert.IsTrue(result == Matrix3.Identity(), "Not an identity");
        }
        
        /// <summary>
        /// Validates that combining transforms yields the expected result
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ValidateAppendTransform2D()
        {
            Point2D[] points = { new Point2D(0, 0), new Point2D(1, 1), new Point2D(-2, -1), new Point2D(-3, 10), new Point2D(5, -2) };

            // rotation and translation
            Pose2D pose = new Pose2D(-1, 5, Math.PI / 6);
            CoordinateTransform2D r = new CoordinateTransform2D(new Pose2D(0, 0, pose.Heading));
            CoordinateTransform2D t = new CoordinateTransform2D(new Pose2D(pose.X, pose.Y, 0));
            CoordinateTransform2D rt = r.Append(t);
            Matrix matrix = new Matrix();
            matrix.Rotate((float)(pose.Heading * MathConstants.Radians2Degrees));
            matrix.Translate((float)pose.X, (float)pose.Y, MatrixOrder.Append);
            Point2D[] expectedPoints = BaselineTransform(matrix, points);
            ValidateTransformResults(rt, points, expectedPoints);
        }

        /// <summary>
        /// Validates the transformation of a given set of points against an expected set of results
        /// </summary>
        /// <param name="transform">The transform to apply</param>
        /// <param name="pointsToTransform">The points to transform (input)</param>
        /// <param name="expectedResults">The expected results</param>
        private static void ValidateTransformResults(CoordinateTransform2D transform, Point2D[] pointsToTransform, Point2D[] expectedResults)
        {
            for (int i = 0; i < pointsToTransform.Length; i++)
            {
                Point2D pointToTransform = pointsToTransform[i];
                Point2D transformedPoint = transform.Transform(pointToTransform);
                if (Math.Abs(transformedPoint.X - expectedResults[i].X) > MathConstants.ErrorEpsilon || Math.Abs(transformedPoint.Y - expectedResults[i].Y) > MathConstants.ErrorEpsilon)
                {
                    throw new Exception(string.Format("{0} is not an expected value after transforming {1}. Expected {2}", transformedPoint, pointToTransform, expectedResults[i]));
                }
            }
        }

        /// <summary>
        /// Validates the transformation of a given set of pose2Ds against an expected set of results
        /// </summary>
        /// <param name="transform">The transform to apply</param>
        /// <param name="pose2DsToTransform">The pose2Ds to transform (input)</param>
        /// <param name="expectedResults">The expected results</param>
        private static void ValidatePose2DTransformResults(CoordinateTransform2D transform, Pose2D[] pose2DsToTransform, Pose2D[] expectedResults)
        {
            for (int i = 0; i < pose2DsToTransform.Length; i++)
            {
                Pose2D pose2DToTransform = pose2DsToTransform[i];
                Pose2D transformedPoint = transform.Transform(pose2DToTransform);
                if (Math.Abs(transformedPoint.X - expectedResults[i].X) > MathConstants.ErrorEpsilon || Math.Abs(transformedPoint.Y - expectedResults[i].Y) > MathConstants.ErrorEpsilon || Math.Abs(transformedPoint.Heading - expectedResults[i].Heading) > MathConstants.ErrorEpsilon)
                {
                    throw new Exception(string.Format("{0} is not an expected value after transforming {1}. Expected {2}", transformedPoint, pose2DToTransform, expectedResults[i]));
                }
            }
        }

        /// <summary>
        /// Provides the baseline for transformations, using the System.Drawing matrix class
        /// </summary>
        /// <param name="transform">The transform as a matrix</param>
        /// <param name="pointsToTransform">The points to transform</param>
        /// <returns>The baseline results</returns>
        private static Point2D[] BaselineTransform(Matrix transform, Point2D[] pointsToTransform)
        {
            PointF[] points = pointsToTransform.Select(p => new PointF((float)p.X, (float)p.Y)).ToArray();
            transform.TransformPoints(points);
            return points.Select(p => new Point2D(p.X, p.Y)).ToArray();
        }

        /// <summary>
        /// Provides the baseline for transformations of Pose2D, using the System.Drawing matrix class
        /// </summary>
        /// <param name="transform">The transform as a matrix</param>
        /// <param name="rotationAngle">Rotation angle</param>
        /// <param name="pointsToTransform">The points to transform</param>
        /// <returns>The baseline results</returns>
        private static Pose2D[] BaselineTransformPose2D(Matrix transform, double rotationAngle, Pose2D[] pointsToTransform)
        {
            PointF[] points = pointsToTransform.Select(p => new PointF((float)p.X, (float)p.Y)).ToArray();
            transform.TransformPoints(points);

            Pose2D[] pose2Ds = new Pose2D[points.Length];
            for (int i = 0; i < pose2Ds.Length; ++i)
            {
                pose2Ds[i] = new Pose2D(points[i].X, points[i].Y, pointsToTransform[i].Heading + rotationAngle);
            }

            return pose2Ds;
        }
    }
}
